import cv2
import numpy as np

#OpenCV定义的结构元素

class ring(object):
    def __init__(self):
        self.Gaussian_Blur = None
        self.medianBlur = None
        self.area = None
        self.param1 = None
        self.param2 = None
        self.minRadius = None
        self.maxRadius = None
        self.Ring_separation_distance = None
        self.Distance_rectangle_circle = None
        self.debug = False
        self.num = 0;
        self.x_agv1 = 0
        self.y_agv1 = 0

    def image_init(self,Gaussian_Blur,median_Blur):
        self.Gaussian_Blur = Gaussian_Blur
        self.medianBlur = median_Blur
    def parameter_init(self,area,param1,param2,minRadius,maxRadius,Ring_separation_distance,Distance_rectangle_circle,debug = False):
        self.area = area
        self.param1 = param1
        self.param2 = param2
        self.minRadius = minRadius
        self.maxRadius = maxRadius
        self.Ring_separation_distance = Ring_separation_distance
        self.Distance_rectangle_circle = Distance_rectangle_circle
        self.debug = debug
    def find(self,frame):
        xy_data = []
        xy_in_data_bit= 0
        xy_data_xmin = []
        xy_data_xmid = []
        xy_data_xmax = []
        len_list = []

        frame1 = frame.copy()
        frame1 = cv2.GaussianBlur(frame1, self.Gaussian_Blur, 0)  # 高斯模糊
        GrayImage = cv2.cvtColor(frame1, cv2.COLOR_BGR2GRAY)
        GrayImage = cv2.medianBlur(GrayImage, self.medianBlur)
        try:
            circles = cv2.HoughCircles(GrayImage, cv2.HOUGH_GRADIENT, 1, 1, param1=self.param1, param2=self.param2, minRadius=self.minRadius,maxRadius=self.maxRadius)
            circles = np.uint16(np.around(circles))
            for i in circles[0, :]:  # 遍历矩阵每一行的数据
                xy_data.append([i[0], i[1]])
            xy_data = sorted(xy_data, key=(lambda x: x[0]), reverse=False)
            if len(xy_data) == 1:
                xy_data_xmin = xy_data.copy()
            for i in range(len(xy_data) - 1):
                if abs(int(xy_data[i][0]) - int(xy_data[i + 1][0])) < self.Ring_separation_distance:
                    if xy_in_data_bit == 0:
                        xy_data_xmin.append(xy_data[i])
                        if len(xy_data) - 1 == (i + 1):
                            xy_data_xmin.append(xy_data[i + 1])
                    elif xy_in_data_bit == 1:
                        xy_data_xmid.append(xy_data[i])
                        if len(xy_data) - 1 == (i + 1):
                            xy_data_xmid.append(xy_data[i + 1])
                    elif xy_in_data_bit == 2:
                        xy_data_xmax.append(xy_data[i])
                        if len(xy_data) - 1 == (i + 1):
                            xy_data_xmax.append(xy_data[i + 1])

                else:
                    xy_in_data_bit += 1

            len_list.append(len(xy_data_xmin))
            len_list.append(len(xy_data_xmid))
            len_list.append(len(xy_data_xmax))


            xadd = 0
            yadd = 0
            if len_list.index(max(len_list)) == 0:
                for i in range(len(xy_data_xmin)):
                    xadd += xy_data_xmin[i][0]
                    yadd += xy_data_xmin[i][1]
                xaver = xadd / (len(xy_data_xmin))
                yaver = yadd / (len(xy_data_xmin))

            if len_list.index(max(len_list)) == 1:
                for i in range(len(xy_data_xmid)):
                    xadd += xy_data_xmid[i][0]
                    yadd += xy_data_xmid[i][1]
                xaver = xadd / (len(xy_data_xmid))
                yaver = yadd / (len(xy_data_xmid))

            if len_list.index(max(len_list)) == 2:
                for i in range(len(xy_data_xmax)):
                    xadd += xy_data_xmax[i][0]
                    yadd += xy_data_xmax[i][1]
                xaver = xadd / (len(xy_data_xmax))
                yaver = yadd / (len(xy_data_xmax))
            xaver = xaver * 10
            yaver = yaver * 10
            if(self.num < 2):
                self.x_agv1 = self.x_agv1 + xaver
                self.y_agv1 = self.y_agv1 + yaver
                self.num = self.num + 1
            else:
                xaver1 = int(self.x_agv1/2)
                yaver1 = int(self.y_agv1/2)
                self.num = 0
                self.y_agv1 = 0
                self.x_agv1 = 0
            if self.debug:
                cv2.circle(frame, (xaver, yaver), 2, (0, 0, 255), 3)
        except Exception as e:
            pass
        try:
            return frame,xaver1,yaver1
        except Exception as e:
            return frame, None, None